﻿
// 2018/6/20: 首个版本
// 2018/7/3: 添加calLaneHeading, calLaneCurvature, calCenterDeparture
// 2018/8/24: 支持v3
// 2018/9/28: 修正大量bug
// 2019/6/10: sortLines仅输出x=[0,10]内含车道线，参数调整
// 2019/6/11: 修正sortLines bug
// 2019/8/8: 根据后轴位置修正STLC/TTLC计算
// 2019/8/28: 修正计算TTLC bug
// 2019/11/22: 修正sortLines bug
// 2020/1/16: 支持v4，移除指标计算代码
// 2020/9/29: 修改public字段为raw字段
// 2021/9/27: 修正calLine
// 2022/10/11: 修正calLaneCurvature
// 2023/10/9: 增加英文注释

#ifndef LANE_FUNC_H
#define LANE_FUNC_H

#include "spadas.h"
#include "lane-sensor-sample-v4.h"

namespace lanecalc
{
	using namespace spadas;
	using namespace lane_sensor_sample_v4;

	enum LaneMode
	{
		LM_Clothoid = 1,
		LM_Mathematic = 2,
	};

	void calLine(Double ox, Double oy, Double yaw, LaneMode modeFirstOrder, LaneMode modeSecondOrder, LaneMode modeThirdOrder, Double values[4]/* in-out */);
	
	void trackLine(LaneSensorSample& sample, Optional<LaneSensorSample> lastSample, UInt& lineIDCount);

	Array<UInt> sortLines(LaneSensorSample& sample); // Output lane lines from left to right inside x=[0,10] / 输出从左到右x=[0,10]内含车道线

	OptionalDouble calLaneWidth(LaneSensorSample& sample, Array<UInt> sortedIndex);
	OptionalDouble calLaneHeading(LaneSensorSample& sample);
	OptionalDouble calLaneCurvature(LaneSensorSample& sample);

	OptionalDouble calLateralVelocity(LaneSensorSample& sample);
	OptionalDouble calCenterDeparture(LaneSensorSample& sample, Array<UInt> sortedIndex);

	void getFeatureLine(LaneSensorSample& sample, Array<UInt> sortedIndex, OptionalInt& firstLeft, OptionalInt& secondLeft, OptionalInt& firstRight, OptionalInt& secondRight);
}

#endif